1. Field of the Invention
The present invention relates to finding desirable paths between specified beginning and ending states of a physical system. More particularly, the invention relates to computing least cost paths for moving physical systems from beginning states to ending states through one or more intermediate states, where the reachable states from each given state are limited by hard constraints.
2. Description of the Related Art
An important concern in many fields of engineering and commerce is to determine the most desirable sequence of steps for transitioning a physical system from a beginning state or configuration to an ending state through one or more intermediate states. For example, the system may be a vehicle traveling between specified points, and the objective is to find the fastest route through a group of intermediate points. Another example is a robotic arm used in automated manufacturing operations. The arm""s productivity on the assembly line depends directly on being able to compute shortest paths through a general parameter space defined by the arm""s degrees of freedom. Further, the system may be a collection of unassembled components of a product, and the objective is to assemble the components in a sequence that requires the least time and materials.
The xe2x80x9ccostxe2x80x9d associated with a path can therefore represent any quantifiable factor associated with the path and defined in terms of physical parameters of the system. For example, if the objective is to determine the shortest path over terrain and around obstacles, then cost represents the Euclidean distance traveled by the vehicle. If the objective is to minimize exposure of a strike aircraft to enemy radar, then cost can represent the accumulated illumination time over the path traveled. Further, it is well known that an optimization problem with objective (cost) function to be minimized generally can be reformulated as an equivalent problem with an objective (profit) function to be maximized.
In optimization problems it is often possible to model various states of the system as discrete points in a state space. For example, if the system is a vehicle traveling between specified locations, then the state can be defined by the physical coordinates (in two or three dimensions) of the vehicle""s location. If the system is a collection of parts being assembled, then the state may be defined as a particular stage of assembly in which certain parts have been joined but not others.
The possible discrete configurations of such a system can be represented by a so-called xe2x80x9cdirected graphxe2x80x9d comprising a set of vertices V={v1, v2, . . . , vM} and a set of edges E={eij} in which eij is an edge from (originating at) vertex vi to (ending at) vj. Here M=|V| is the number of vertices in the graph. Usually E contains edges for only certain values of i and j. The number N of edges in E may be as large as M(Mxe2x88x921) for a fully connected, directed graph and generally is much larger than M. Often N is a small multiple of M (such as N=4M, for example).
Associated with each edge eij in E is a cost cij representing the cost for the system to transition from vertex (state) vi to vj. If the system is a vehicle, the state is the geographical position of the vehicle, and the cost associated with routing the vehicle is simply proportional to the total distance traveled, then the problem can be modeled with cij=d(vi, vj), where d(,) is Euclidean distance. In the explanation presented here the cost associated with each edge is assumed for simplicity to be a positive value. Persons of ordinary skill in the art will appreciate, however, that the method of the present invention can be implemented in the case of negative costs, as well.
Each vertex in the graph G corresponds to a different state of the system, such as a different location through which a vehicle can travel. The edges serve to link the vertices together and thus represent transitions between the various states. In a directed graph the edges are directional; that is, each edge provides a transition between two states in a single direction. If it is possible to move from either of two states to the other, then the graph includes one edge for each direction.
Computational methods using graphs to solve least-cost path problems have existed for decades. Dijkstra""s algorithm is a powerful technique that can be implemented to identify a sequence of edges connecting a given source vertex, vs, to a given terminal (or sink) vertex, vt, such that the sum of the costs associated with the edges in the path is a minimum. The computational complexity of Dijkstra""s algorithm scales as O(|V|log|V|+E) and thus is very efficient. This efficiency arises in part, at least, because the edges are selected independently. In other words, each edge along the path is selected from among all edges exiting the given vertex, without regard for the edge on which the system arrived at that vertex.
Unfortunately, many types of optimization problems involve restrictions to prohibit sequences of transitions that are incompatible with the physical limitations of the system. For example, if the optimization problem involves routing a vehicle, a path containing a pair of edges requiring the vehicle to make a 120-degree turn might be infeasible because the vehicle is incapable of making such a sharp turn. Such limitations arise in a wide variety of other least-cost path problems. When routing ground vehicles over free terrain (not on prepared roadways), restrictions often must be placed on changes in vertical angles in order to provide sufficient clearance for the front or back of the vehicle. The routing of a commercial aircraft must consider the sharpness of turns in terms of both stress on the airframe and comfort level of the passengers. For a robotic arm, sharp angular motions can incur joint stresses that can lead to material fatigue, especially when the arm performs a repetitive task as in a manufacturing operation.
Because it relies on independent selection of graph edges to construct a path, Dijkstra""s algorithm cannot be used directly for systems involving xe2x80x9chardxe2x80x9d constraints on state transitions. Here a constraint is xe2x80x9chardxe2x80x9d when it eliminates certain edge sequences from consideration, rather than merely imposing additional costs (xe2x80x9cpenaltiesxe2x80x9d) for using such edge sequences. For example, a constraint on turn sharpness will appear in the model as a limitation on the range of angles between the edge used to enter a vertex and the edges that can be used to exit the vertex.
Generalized forms of Dijkstra""s algorithm have occasionally been suggested to allow costs to be associated with edge sequences (such as turns between pairs of edges). Such approaches involve applying a suitable transformation to the graph G to produce a transformed graph Gxe2x80x2 comprising vertices vxe2x80x2ij representing edges eij from vertex vi to vertex vj in graph G. Each edge exe2x80x2ijk in Gxe2x80x2 represents, for example, a transition from a vertex vxe2x80x2ij, vxe2x80x2jk and thus corresponds to a pair of edges eij, ejk in G. A cost associated with the edge eijk in Gxe2x80x2 therefore corresponds to a cost (or penalty) for traversing the sequence of edges eij, ejk. The constrained optimization problem on G is thereby transformed into an unconstrained optimization problem on Gxe2x80x2.
In theory, Dijkstra""s algorithm could be implemented for the problem on Gxe2x80x2. Gxe2x80x2 is much larger (number of vertices and edges) than G, however, and in fact the computational complexity of the transformed problem scales as O(|E| |V|), i.e., O(NM). This fact has made the transformed graph approaches so time consuming as to be completely useless for any problem of practical size.
A need has therefore existed for a method by which least-cost paths can be determined under hard constraints. Such a method should make it possible to compute (in practical amounts of time) constrained minimum cost routes for vehicles on problems of practical size. Preferably, such a method should be applicable to any form of xe2x80x9ccostxe2x80x9d that is quantifiable in terms of measurable system parameters and can be represented by positive values.
The present invention addresses these and other needs by providing a method, system, and software for determining least-cost paths under hard constraints such as turn constraints. The notion of cost is extremely general. As a special case the present invention is able to determine the shortest path (i.e., where cost represents Euclidean distance) over terrain and around obstacles under the condition that no turns in the path exceed a given angle. Arbitrary turn penalties on permitted turn angles can also be included. The invention can be used to find a path for a strike aircraft that minimizes exposure to enemy radar under the condition that no horizontal turn exceeds a given angle and no vertical ascent or descent exceeds a different given angle. Constraints on variables other than angles between consecutive segments of a Euclidean path may also be enforced using the present invention.
The invention achieves these results through two basic procedures. First, the constrained least cost path problem for the physical system is transformed into an unconstrained problem, which can be solved by an implementation of Dijkstra""s algorithm. Prior approaches have applied Dijkstra""s algorithm to transformed optimization problems, but have been extremely inefficient in practice. The present invention overcomes this drawback by exploiting special relationships between a transformation incorporating hard constraints and data structures that can be suitably tailored to avoid the inefficiencies of prior approaches.
In a first aspect, the present invention provides for finding a least-cost path from a source vertex to a sink vertex in a directed graph representing a plurality of configurations of a physical system. Each edge of the graph has a corresponding cost associated therewith and each vertex of the graph has a corresponding predetermined condition defining for the vertex a reachable range of adjacent vertices of the graph. A method provided by this aspect of the invention comprises retrieving from a priority queue a minimum priority data item indicating a corresponding current vertex of the graph and having an associated priority. The method also comprises identifying from an adjacency list each vertex of the graph adjacent to the current vertex and reachable therefrom in accordance with the predetermined condition corresponding to the current vertex when the current vertex is not the sink vertex, each identified vertex being indicated by a corresponding data item to be inserted into the priority queue with an associated priority.
In a second aspect, the present invention provides for finding a constrained least-cost path from a source vertex to a sink vertex in a directed graph representing a plurality of configurations of a physical system and comprising plural vertices and plural edges between respective vertices. A method provided by this second aspect of the invention comprises retrieving and deleting from a priority queue a minimum priority data item indicating a corresponding current edge of the graph and having an associated priority, the current edge originating at a first vertex of the graph and ending at a second vertex of the graph. The method also comprises identifying from a searchable data structure each edge of the graph adjacent to the current edge and reachable therefrom in accordance with a predetermined condition when the second vertex is not the sink vertex, each identified edge being indicated by a corresponding data item to be inserted into the priority queue with an associated priority.